import Project.Main as Main
import robo3pi as a
import time
import Project.Tracker.TrackerInterface as t
import Project.Utilities.Utilities as u
import math

global ser

RIGHT_WHEEL_OFFSET = Main.RIGHT_WHEEL_OFFSET
LEFT_WHEEL_OFFSET = Main.LEFT_WHEEL_OFFSET
ROTATION_TIME_360 = Main.ROTATION_TIME_360
EPSILON = Main.EPSILON
VELOCITY = Main.VELOCITY

centerPoints1 = []
centerPoints2 = []
azimuthArr= []


def moveInLineAdapter(id, distance):
    while (0 != distance):
        if (Main.DIST_PARITY < abs(distance)):
            sign = distance / abs(distance)
            dist = Main.DIST_PARITY * sign
        else:
            dist = distance
        moveInLine (id, dist)
        distance -= dist
    
    
def moveInLine (id, distance):
    
    RIGHT_WHEEL_OFFSET = Main.RIGHT_WHEEL_OFFSET
    LEFT_WHEEL_OFFSET = Main.LEFT_WHEEL_OFFSET

    if abs(distance) < Main.DISTANCE_EPSILON:
        return
    
    
    if distance < 0:
        sign = -1
    else:
        sign = 1
        
    #print("distance is: " + str(distance))
    distance = 1.07 * distance / 100
    
    
    
    velocity = VELOCITY * 255
    if (velocity < 15):
        first_part = 1
        sec_part = 0
    else:
        first_part = 0.3
        sec_part = 0.7
          
    movingTime = abs(distance * first_part /VELOCITY)
    
    if (abs(movingTime) == 0):
        print ("Time has incorrect value. System ABORTED :)")
        return 
     
    a.move(id, sign * (velocity + LEFT_WHEEL_OFFSET), sign * (velocity + RIGHT_WHEEL_OFFSET)) #TODO: Try and avoid offsets
    time.sleep(movingTime)
    distance = distance * sec_part *  0.2
    
    velocity_delta = velocity - 15
    for i in range (0,5): 
        velocity = velocity - (velocity_delta/5)
        leftMovingTime = abs(distance/(velocity/255))
        a.move(id, sign * (velocity + LEFT_WHEEL_OFFSET), sign * (velocity  + RIGHT_WHEEL_OFFSET))
        time.sleep(leftMovingTime)
        #print ("velocity is now " + str (velocity))
        #print ("left time is now " + str(leftMovingTime))
 

    a.stop(id)
    
    time.sleep(EPSILON)
    return 1
    
    
    
       
def rotateByAngle(id, angle):
    
    if abs(angle) < Main.ANGLE_EPSILON:
        return
    
    #TODO: using default velocity value, consider using parameter
    rotationTime = abs ( (angle / 360.0) * Main.ROTATION_TIME_360 )
    
    #print ("Rotation time is: " + str(rotationTime))
    
    if angle > 0:
        motorSpeed = [-30, 30]
    else:
        motorSpeed = [30, -30]
    
    a.move(id, motorSpeed[0], motorSpeed[1])   
      
    time.sleep(rotationTime) #TODO: our own sleep
    #print ("finished sleep")
    a.stop(id) 
    #print ("finished stop")
    time.sleep(EPSILON)
    return 1
 
def moveInRectangle(id, edgeLength, seconds): 
    
    for i in range (0, 4):
        moveInLine(id, edgeLength, seconds/4)
        #print ("finished move")        
        rotateByAngle(id, 90)
         
    return 1

def moveInPolygon(id, numOfEdges, edgeLength, seconds):
    
    if (numOfEdges == 3):
        angle = 60
    elif (numOfEdges == 4):
        angle = 90
    elif (numOfEdges == 5):
        angle = 108
    elif (numOfEdges == 6):
        angle = 120
    elif (numOfEdges == 7):
        angle = 128.5
    elif (numOfEdges == 8):
        angle = 135
        
    angle = 180 - angle
        
    for i in range (0, numOfEdges):
        
        moveInLine(id, edgeLength, seconds/numOfEdges)
        #print ("finished move")        
        time.sleep(EPSILON)
        rotateByAngle(id, angle)
        
    
    return 1
    
def normalization ( x , y , width):
    nx = (x/width)*(2)-1
    ny = (y/width)*(2)-1
    point = [ nx , ny ]
    return point
    
    
def moveInLineOld(id, distance, seconds):
    error = -1
    velocity = (distance/seconds) * 255
    
    print ("Velocity is: " + str(velocity))
    
    if (abs(velocity) > 255 or abs(velocity) < 20):
        print ("Velocity has incorrect value. System ABORTED :)")
        return error
    
    a.move(id, velocity + LEFT_WHEEL_OFFSET, velocity + RIGHT_WHEEL_OFFSET) #TODO: Try and avoid offsets
    time.sleep(seconds)
    a.stop(id)
    time.sleep(EPSILON)

# Distance in meters, Velocity in M/S (Meters per second)
def oldMoveInLine(id, distance, velocity):
    error = -1
    
    if (velocity > 1 or velocity == 0):
        print ("Velocity is too big")
        return error
    
    movingTime = (distance/velocity)
    velocity = velocity * 255 # Transform velocity to robot values
    
    print ("Velocy is " + str(velocity))
    print ("Movement time is: " + str(movingTime))
    
    if (abs(movingTime) == 0):
        print ("Time has incorrect value. System ABORTED :)")
        return error
    
    a.move(id, velocity + LEFT_WHEEL_OFFSET, velocity + RIGHT_WHEEL_OFFSET) #TODO: Try and avoid offsets
    time.sleep(movingTime)
    a.stop(id)
    time.sleep(EPSILON)
    
    
    # the function suppose to move all robots together forwards for ten centimeters and save the robots new 
    #center point and move them back to the starting point. now for each robot we have two points and we can calculate the direction
    
def getAzimuth ():
    i = 0
    azimuthArr = []
    centerPoints2 = []
    centerPoints1 = []
    
    time.sleep(Main.SLEEP_TIME_COMMANDS)     
    robotArray = t.trace()
    for robot in robotArray:
        moveInLine(str(robot.Id + Main.OFFSET_ROBOTS), -3) 
        print(str(robot.Id))
    
    time.sleep(Main.SLEEP_TIME_COMMANDS)     
    robotArray = t.trace()
    for robot in robotArray:
        centerPoints2.append(robot.center)
        moveInLine(str(robot.Id + Main.OFFSET_ROBOTS), 3)

        
    robotArray = t.trace()
    for robot in robotArray:
       centerPoints1.append(robot.center)
    
    for i in range (0, len(robotArray)):
            
            azimuthArr.append(180 - u.findSlope( centerPoints1[i] , centerPoints2[i]))
          #  azimuthArr.append( u.findSlope( centerPoints1[i] , centerPoints2[i]))
            print ( str(robot.Id) + "= id " + str(azimuthArr[i]) + " = azimuth")
            
    return azimuthArr


 
    
        
    
    
    
    
    
    















